// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "serial/serial.h"
#include "std_msgs/msg/float64.hpp"
#include <chrono>
#include <thread>

serial::Serial ros_ser;
using namespace std::chrono_literals;

uint8_t data_l1[2];
uint8_t data_l2[2];
uint8_t data_l3[2];
uint8_t data_l4[2];

// 3.自定义节点类；
class Arm_Adc : public rclcpp::Node
{
public:
    Arm_Adc() : Node("arm_adc_node_cpp")
    {
        // 创建定时器
        timer_ = this->create_wall_timer(0.1s, std::bind(&Arm_Adc::on_timer, this));
        joint2_pub = this->create_publisher<std_msgs::msg::Float64>("joint2_p", 10);
        joint3_pub = this->create_publisher<std_msgs::msg::Float64>("joint3_p", 10);
        joint2_pressure_pub = this->create_publisher<std_msgs::msg::Float64>("joint2_pressure", 10);
        joint3_pressure_pub = this->create_publisher<std_msgs::msg::Float64>("joint3_pressure", 10);
    }

private:
    // 创建定时发布
    rclcpp::TimerBase::SharedPtr timer_;
    // 创建关节二位置信息发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint2_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint3_pub;
    // 创建压力信息发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint2_pressure_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint3_pressure_pub;

    // 定时器回调函数
    void on_timer()
    {
        auto joint2_p = std_msgs::msg::Float64();
        auto joint3_p = std_msgs::msg::Float64();
        auto joint2_pressure = std_msgs::msg::Float64();
        auto joint3_pressure = std_msgs::msg::Float64();

        float joint2_v;
        float joint3_v;
        float joint2_pv;
        float joint3_pv;

        uint8_t senddata[8] = {0x01, 0x04, 0x00, 0x00, 0x00, 0x04, 0xf1, 0xc9};
        ros_ser.write(senddata, 8);
        //  等待响应
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        uint8_t data[13] = {0};
        ros_ser.read(data, 13);
        // 打印data数组的十六进制值
        // RCLCPP_INFO(this->get_logger(), "Received data:");
        for (int i = 0; i < 13; ++i)
        {
            // RCLCPP_INFO(this->get_logger(), "0x%02X", data[i]);
        }

        // ----------一号通道数据解析----------
        data_l1[0] = data[3];
        data_l1[1] = data[4];
        // RCLCPP_INFO(this->get_logger(), "0x%02X 0x%02X \n", data_l1[0], data_l1[1]);
        int32_t joint2_hex = (static_cast<uint16_t>(data_l1[0] << 8) | data_l1[1]);
        int joint2_dec = static_cast<int>(joint2_hex);
        // printf("方式二：%d \n", joint2_dec);
        joint2_v = ((float)joint2_dec) / 65535 * 5;
        joint2_p.data = (joint2_v - 0.5) * 375;
        joint2_pub->publish(joint2_p);
        printf("二号测算的位移为%f \n", (joint2_v - 0.5) * 375);

        // ----------二号通道数据解析----------
        data_l2[0] = data[5];
        data_l2[1] = data[6];
        // RCLCPP_INFO(this->get_logger(), "0x%02X 0x%02X \n", data_l2[0], data_l2[1]);
        int32_t joint3_hex = (static_cast<uint16_t>(data_l2[0] << 8) | data_l2[1]);
        int joint3_dec = static_cast<int>(joint3_hex);
        // printf("方式二：%d \n", joint3_dec);
        joint3_v = ((float)joint3_dec) / 65535 * 5;
        joint3_p.data = (joint3_v / 10) * 1000 - 51.62;
        joint3_pub->publish(joint3_p);
        printf("三号测算的位移为%f \n", (joint3_v / 10) * 1000 - 51.62);

        // ----------三号通道数据解析----------

        data_l3[0] = data[7];
        data_l3[1] = data[8];
        // RCLCPP_INFO(this->get_logger(), "0x%02X 0x%02X \n", data_l3[0], data_l3[1]);
        int32_t joint2_phex = (static_cast<uint16_t>(data_l3[0] << 8) | data_l3[1]);
        int joint2_pdec = static_cast<int>(joint2_phex);
        // printf("方式二：%d \n", joint2_pdec);
        joint2_pv = ((float)joint2_pdec) / 65535 * 5;
        joint2_pressure.data = (joint2_pv - 0.5) * 15;
        joint2_pressure_pub->publish(joint2_pressure);
        printf("四号测算的压力为%f MPa\n", joint2_pressure.data);
        // ----------四号通道数据解析----------
        data_l4[0] = data[9];
        data_l4[1] = data[10];
        // RCLCPP_INFO(this->get_logger(), "0x%02X 0x%02X \n", data_l4[0], data_l4[1]);
        int32_t joint3_phex = (static_cast<uint16_t>(data_l4[0] << 8) | data_l4[1]);
        int joint3_pdec = static_cast<int>(joint3_phex);
        // printf("方式二：%d \n", joint3_pdec);
        joint3_pv = ((float)joint3_pdec) / 65535 * 5;
        joint3_pressure.data = (joint3_pv - 0.5) * 15;
        joint3_pressure_pub->publish(joint3_pressure);
        printf("五号测算的压力为%f MPa\n", joint3_pressure.data);

        // RCLCPP_INFO(this->get_logger(), "Combined decimal value:");
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 连接串口
    ros_ser.setPort("/dev/ttyS0");
    // 串口波特率
    ros_ser.setBaudrate(9600);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ros_ser.setTimeout(to);
    try
    {
        ros_ser.open();
    }
    catch (serial::IOException &e)
    {
        std::cout << "unable to open" << std::endl;
        return -1;
    }
    if (ros_ser.isOpen())
    {
        std::cout << "-----open------" << std::endl;
    }
    else
    {
        return -1;
    }
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Arm_Adc>());
    // 5.资源释放
    rclcpp::shutdown();
    // 关闭串口
    ros_ser.close();
    return 0;
}